package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.spot.DemoSwitchListener;
import com.grt192.mechanism.spot.SLight;
import com.grt192.prototyper.PrototyperUtils;
import com.grt192.sensor.DemoJoystick;
import com.grt192.sensor.spot.GRTDemoSwitch;
import com.grt192.sensor.spot.NetSensor;
import com.grt192.utils.Util;
import com.iaroc.irobot.IRobotCreate;
import com.iaroc.irobot.IRobotCreateConstants;

/**
 *
 * @author ajc
 */
public class DriverDemo extends GRTObject implements Macro,
        TelemetryPorts,
        DemoSwitchListener,
        SensorChangeListener {

    public DriverDemo() {
        setPrinting(true);
    }

    public int execute() {
        initLights();
        initActuator();
        startSensors();
        this.start();
//        sensorLoop();
        while (true) {
            Util.sleep(1000);
        }
    }

    /*
     * 6 lights available:
     * compass, gyro, create
     * 1 for each
     */
    private SLight[] lights = SLight.get();
    private int createLight = 2;
    private int switchLights[] = new int[]{5, 6};
    private int joystickLight = 7;

    private void initLights() {
        log("Initializing lights...");
        lights[createLight].rawColor(GRTDemoLED.Color.ORANGE);
        for (int i = 0; i < switchLights.length; i++) {
            lights[switchLights[i]].rawColor(GRTDemoLED.Color.ORANGE);
        }
        log("Lights initialization success!");

    }
    IRobotCreate create;

    public void initActuator() {
        log("Initializing create....");
        lights[createLight].rawColor(GRTDemoLED.Color.ORANGE);
        create = new IRobotCreate(false);
        lights[createLight].rawColor(GRTDemoLED.Color.GREEN);
        log("Create initialization successful!");
    }
    GRTDemoSwitch[] switches;
    NetSensor joystick;

    public void startSensors() {
//        log("Initializing switches...");
//        switches = new GRTDemoSwitch[2];
//        for (int i = 0; i < switches.length; i++) {
//            switches[i] = new GRTDemoSwitch(i, 50, "Swtich" + 0);
//            switches[i].addSwitchListener(this);
//            switches[i].start();
//        }
//        log("Switch initialization success!");
        log("Initializing Joystick...");
        joystick = new NetSensor(PrototyperUtils.getAddress(1, lights[joystickLight]), JOYSTICK_PORT);
        joystick.addSensorChangeListener(this);
        joystick.start();
        log("Joystick initialization success!");
    }

//    private String getHost2() {
//        lights[joystickLight].rawColor(GRTDemoLED.Color.RED);
//        String host2 = PrototyperFactory.getPrototyper().getHost(2).getAddress();
//        while (host2 == null) {
//            host2 = PrototyperFactory.getPrototyper().getHost(2).getAddress();
//            lights[joystickLight].blinkToBlack();
//            Util.sleep(1000);
//        }
//        lights[joystickLight].rawColor(GRTDemoLED.Color.GREEN);
//        return host2;
//    }
    public void sensorLoop() {
        while (true) {
            if (create != null) {
                create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6);
                log("Angle: " + create.getAccumulatedAngle());
            }

            Util.sleep(50);
        }
    }

    public void switchPressed(GRTDemoSwitch source) {
//        if (source == switches[0]) {
//            create.spinRight();
//            lights[switchLights[0]].rawColor(GRTDemoLED.Color.GREEN);
//        } else if (source == switches[1]) {
//            create.spinLeft();
//            lights[switchLights[1]].rawColor(GRTDemoLED.Color.GREEN);
//
//        }
    }

    public void switchReleased(GRTDemoSwitch source) {
//        if (source == switches[0]) {
//            lights[switchLights[0]].rawColor(GRTDemoLED.Color.NONE);
//        } else if (source == switches[1]) {
//            lights[switchLights[1]].rawColor(GRTDemoLED.Color.NONE);
//        }
//        create.stop();
    }

    private static double convertPercent(double intMin, double intMax, double outMin, double outMax, double in) {
        /*to get a percent:
         * find out how the number, shifted to zero, compares to the range shifted to zero.
         */
        double percent = (in - intMin) / (intMax - intMin);
        double newValue = (percent * (outMax - outMin)) + outMin;
        return newValue;

    }

    private static double getPercent(double min, double max, double in) {
        return (in - min) / (max - min);
    }

    private static double percentToRange(double min, double max, double percent) {
        return (percent * (max - min)) + min;
    }
    boolean running = false;

    public void run() {
        log("Starting drive loop...");
        running = true;
        while (running) {
            drive();
            Util.sleep(50);
        }
    }

    public void drive() {
        int yV = getyV(y);
        int yX = getxV(x);
        if (create != null) {
            create.driveDirect(
                    limit(-500, 500, (yV - yX)),
                    limit(-500, 500, (yV + yX)));
        }
    }

    public static int limit(int min, int max, int in) {
        if (in < min) {
            return min;
        }
        if (in > max) {
            return max;
        }
        return in;

    }

    private int getyV(double gravity) {
        double percent = gravity / 1.5;
        double mult = percent <= 0 ? -1.0 : 1.0;
        percent *= percent * mult;
        return (int) (500 * percent);
    }

    private int getxV(double gravity) {
        double percent = gravity / 1.5;
        double mult = percent <= 0 ? -1.0 : 1.0;
        percent *= percent * mult;
        return (int) (500 * percent);

    }
    double x = 0;
    double y = 0;
    double deadzone = .2;

    public void sensorStateChanged(SensorEvent e, String key) {
//        if (key.equals(DemoJoystick.X)) {
//            log("joystick:" + e.getData(key));
//            if (Math.abs(e.getData(key)) > .25) {
//                int speed = (int) convertPercent(-1.5, 1.5, -500, 500, e.getData(key));
//                log("Driving:" + speed);
//                create.driveDirect(-speed, speed);
////                create.driveDirect(speed, speed);
//            } else {
//                create.stop();
//            }
//        }
//
        if (key.equals(DemoJoystick.X)) {
//            x = e.getData(key);
            if (Math.abs(e.getData(key)) > deadzone) {
                x = e.getData(key);
            } else {
                x = 0;
            }
        } else if (key.equals(DemoJoystick.Y)) {
            if (Math.abs(e.getData(key)) > deadzone) {
                y = e.getData(key);

            } else {
                y = 0;
            }
        }

//        if (key.equals(DemoJoystick.Y)) {
//            y = e.getData(key);
//        }

//        if (Math.abs(y) > deadzone) {
//        int speed = getSpeed(y);
//        if (create != null) {
//            create.driveDirect(speed, speed);
//        }
//        }else{
//            if (create != null) {
//                create.stop();
//            }
//        }


        /**
         * Conversion to two tankdrive percentages:
         * 0, 1.5:      100,   100 
         * 0, -1.5:     -100,   -100
         * 1.5, 0:      -100,   100
         * 
         */
//        if (Math.abs(y) > deadzone) {
//            double percent = getPercent(-1.5, 1.5, y);
//            int speed = (int) percentToRange(-500, 500, percent * percent);
//            create.driveDirect(speed, speed);
//        } else {
//            create.stop();
//        }
//        double percent = getPercent(-1.5, 1.5, y);
//        int speed = (int) percentToRange(-500, 500, percent * percent);
//        create.driveDirect(speed, speed);
//        if (key.equals(DemoJoystick.Y)) {
//            log("joystick:" + e.getData(key));
//            if (Math.abs(e.getData(key)) > deadzone) {
//                int speed = getSpeed(e.getData(key));
//                log("Driving:" + speed);
//                if (create != null) {
//                    create.driveDirect(speed, speed);
//                }
//            } else {
//                if (create != null) {
//                    create.stop();
//                }
//            }
//        }
//        
    }
}
